#ifndef _TRACKING_H
#define _TRACKING_H


#include <opencv2/opencv.hpp>
#include "Basic Structure.h"
#include "keyframe.h"
#include "Solve3D.h"
#include "misc.h"
#include <future>
#include <thread>
#include <chrono>
#include "BRIEF.h"
#include <mutex>
#include "BoWWrapper.hpp"
#include "Frame.hpp"
#include "KalmanF.hpp"
#include "Motion_Model.hpp"


namespace markerAR
{
    
class Tracking
{
public:
    typedef enum Tracking_State{
        Tracking_Success,
        Tracking_Lost,
        Not_Initialise,
    }Tracking_State;
    
public:
    Tracking();
    ~Tracking();
    void init(const Pose& C,const Marker*marker, const Keyframe* k,const Eigen::Matrix3d &H,double timeStamp);
    bool tracking(const Mat&img,double timeStamp);
    Tracking_State getState(){
        std::lock_guard<std::mutex> lock(mutex_state);
        return state;};
    const std::vector<Pose>& getC(){
        return now->C;
    };
    const std::vector<Eigen::Matrix4d>& getRenderP();
    bool tryAddMarker(const Mat&img,const QueryResult&result);
    Mat debugImg;
    void OutputTrajectory(const std::string &fileName);
    

private:
    bool tryAddKeyframe();
    Tracking_State state;
    const Keyframe* currentKeyFrame;
    Ptr<Frame> now;
    Keyframe candidate;
    Mat maskCandidate;
    Mat identity;
    bool addKeyframeFinished;//if adding keyframe has finished
    bool startFlag;
    int trackID;//the tracking marker ID
    std::vector<Eigen::Matrix4d> P;
    std::vector<Feature*> transformPoints;
    std::vector<pair<double,Pose>> trajectory;
    std::vector<Eigen::Matrix<double,6,1>> kfOutput;
    std::mutex mutex_tracking;
    std::thread* thread_addKeyframe;
    std::mutex mutex_state;
    MotionFilter kf;
    Motion_Model mm;
};


}


#endif 